MoveIt! tf2 migration #830

IanTheEngineer · 2018-04-09T01:50:18Z

This will resolve #745 and complete a checkbox for Release to Melodic.

This is the migration from tf to tf2 required for ROS2. It depends on ros/geometry2#292 for a handful of additional conversions. It breaks API compatibility, so it can only be merged into the melodic-devel branch.

This PR depends on the following upstream API changes for Melodic:
ros/geometry#163 (Exposing tf2_ros::Buffer object from TransformListener)
ros/geometry2#292 (tf2 Type conversions)
ros/geometry2#294 (Additional tf2 Type conversions)
ros-visualization/rviz#1236 (Exposing tf2 without ROS deprecation warnings)

Highlights:

All type conversions now depend on geometry2 ROS packages, rather than geometry
Removed all boost::shared_ptr<tf::TransformListener> from the API, and replaced them with std::shared_ptr<tf2_ros::Buffer>'s. I figured boost was only kept to preserve API, but please correct me if that is a bad assumption.
Piped tf2_ros::Buffer's everywhere where tf::TransformListeners were used

Things left TODO:

Finish converting moveit_ros/robot_interaction/src/robot_interaction.cpp
Finish converting moveit_ros/perception/mesh_filter/src/transform_provider.cpp
Figure out a better method for retrieving RViz' internal tf2_ros::Buffer instance, as RViz APi only gives access to the boost::shared_ptr<tf::TransformListener> instance:
Migrating RViz DisplayContext to tf2 ros-visualization/rviz#1215
Fix one rather large bug likely caused by my work around to the above point: when wiggling around the interactive markers in RViz, I can periodically get in a state where the marker detaches from the robot's end effector, and this error prints out:

[ERROR] [1523131812.122517887]: Cannot transform from frame 'base' to frame '/base' (no TF instance provided)
[ERROR] [1523131812.148109554]: Cannot transform from frame 'base' to frame '/base' (no TF instance provided)
[ERROR] [1523131812.179335945]: Cannot transform from frame 'base' to frame '/base' (no TF instance provided)

My solution was to just pass in a null Buffer to the move_group when we would have used RViz's TransformListener. This almost seems to work, aside for the bug.

Checklist

Code is (sort-of) auto formatted using clang-format, but I ignored some reformats that looked strange. I'll run it again after review changes.
TBD: Extended the tutorials / documentation, if necessary reference
TBD: Created tests, which fail without this PR reference
As this breaks API/ABI, this PR is melodic-devel only

IanTheEngineer · 2018-04-09T14:24:59Z

travis-ci won't be happy as this PR depends on ros/geometry2#292. I'm open to recommendations on how to remedy that, if necessary.

v4hn · 2018-04-10T05:22:23Z

Just work with it locally until ros/geometry2#292 is merged upstream. We will not merge this request earlier anyway...

davetcoleman

Huge PR!!

davetcoleman · 2018-04-11T04:55:58Z

moveit_core/collision_detection/src/collision_tools.cpp

-  tf::pointEigenToMsg(contact.pos, msg.position);
-  tf::vectorEigenToMsg(contact.normal, msg.normal);
+  msg.position = tf2::toMsg(contact.pos);
+  // FIXME: there really should be a conversion for


// TODO(IanTheEngineer)

davetcoleman · 2018-04-11T04:56:35Z

moveit_core/robot_trajectory/src/robot_trajectory.cpp

@@ -275,8 +275,12 @@ void robot_trajectory::RobotTrajectory::getRobotTrajectoryMsg(moveit_msgs::Robot
      trajectory.multi_dof_joint_trajectory.points[i].transforms.resize(mdof.size());
      for (std::size_t j = 0; j < mdof.size(); ++j)
      {
-        tf::transformEigenToMsg(waypoints_[i]->getJointTransform(mdof[j]),
-                                trajectory.multi_dof_joint_trajectory.points[i].transforms[j]);
+        // FIXME: there should be a


davetcoleman · 2018-04-11T04:57:18Z

moveit_planners/chomp/chomp_interface/include/chomp_interface/chomp_planning_context.h

@@ -69,8 +67,6 @@ class CHOMPPlanningContext : public planning_interface::PlanningContext
 private:
  CHOMPInterfacePtr chomp_interface_;
  moveit::core::RobotModelConstPtr robot_model_;
-
-  boost::shared_ptr<tf::TransformListener> tf_;


davetcoleman · 2018-04-11T04:57:58Z

moveit_ros/move_group/src/move_group_capability.cpp

-    context_->planning_scene_monitor_->getTFClient()->getLatestCommonTime(pose_msg.header.frame_id, target_frame,
-                                                                          common_time, &error);
+    int pf = context_->planning_scene_monitor_->getTFClient()->_lookupFrameNumber(pose_msg.header.frame_id);
+    int cf = context_->planning_scene_monitor_->getTFClient()->_lookupFrameNumber(target_frame);


better var names

davetcoleman · 2018-04-11T04:58:43Z

moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp

@@ -88,7 +90,9 @@ bool PointCloudOctomapUpdater::setParams(XmlRpc::XmlRpcValue& params)

 bool PointCloudOctomapUpdater::initialize()
 {
-  tf_ = monitor_->getTFClient();
+  tf_buffer_.reset(new tf2_ros::Buffer());
+  // FIXME(imcmahon) should the TransformListener use the private_nh_, root_nh_ or create it's own NodeHandle?


davetcoleman · 2018-04-11T05:00:19Z

moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp

+      // FIXME!(imcmahon) this forces the Planning Scene Monitor to allocate a new tf2_ros::Buffer
+      // and tf2_ros::TransformListener  on each invocation. These instances are properly deleted on exit,
+      // but it would be better to remove the null shared pointer once tf2_ros::Buffer is exposed from
+      // RViz with something like context_->getFrameManager()->getTFClientPtr()


IanTheEngineer · 2018-04-20T20:44:32Z

Commit 2da2c8d depends on ros-visualization/rviz#1224, and resolves the two main issues when dealing with transforms from RViz: the crashes no longer manifest.

IanTheEngineer · 2018-04-20T20:45:30Z

moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp

      move_group_.reset(new moveit::planning_interface::MoveGroupInterface(
-          opt, std::shared_ptr<tf2_ros::Buffer>(), ros::WallDuration(30, 0)));
+          opt, context_->getFrameManager()->getTFBufferPtr(), ros::WallDuration(30, 0)));


This depends on ros-visualization/rviz#1224

tfoote · 2018-04-27T22:30:08Z

@IanTheEngineer let me know when you've pushed everything to geometry2 and I can get a new release out in melodic

IanTheEngineer · 2018-04-30T18:53:58Z

@tfoote all the changes that I need are now merged into geometry2, so I think you're good to go in releasing to Melodic. Thanks!

IanTheEngineer · 2018-05-03T17:42:35Z

@davetcoleman and @v4hn I think this PR should build cleanly now that the dependencies have been merged into Melodic. However, the pull request builder is using Kinetic. How do I update the builder to use the right distro?

davetcoleman · 2018-05-03T20:55:03Z

For now I think we'll need to go without the pull request builder, I have not had time to set this up yet.

Can you remove this from the first description if its ready?

Note: This PR is not ready for merge, but ready for discussion and feedback. I will rebase the 20+ commits down to a small, sensible number.

You have an unfinished TODO in the first description:

Finish converting moveit_ros/perception/mesh_filter/src/transform_provider.cpp

These are ready?

IanTheEngineer · 2018-05-03T21:40:09Z

Can you remove this from the first description if its ready?

Note: This PR is not ready for merge, but ready for discussion and feedback. I will rebase the 20+ commits down to a small, sensible number.

I can rebase this down now, but I typically do this right before a merge, after the reviews are made and feedback is incorporated. Keeping them separate helps in viewing the history of the PR, but I admit 40 commits is an unwieldy number.

You have an unfinished TODO in the first description:
Finish converting moveit_ros/perception/mesh_filter/src/transform_provider.cpp
These are ready?

Yep. All of the code is ready. I'll update the checkbox accordingly.

rhaschke · 2018-05-03T21:42:33Z

Please wait with squashing. I'm currently reviewing...

IanTheEngineer · 2018-05-03T21:47:04Z

@rhaschke gotcha. I'll hold off on the squash until I have sufficient reviews.

rhaschke

Generally I approve these changes. @IanTheEngineer, thanks a lot for this great work! Some minor remarks inline.

@tfoote This PR uses the methods _addTransformsChangedListener and _getLatestCommonTime of tf2::BufferCore, which are declared "not to be used anymore". What are the recommended alternatives?

rhaschke · 2018-05-03T21:00:32Z

moveit_ros/perception/mesh_filter/src/transform_provider.cpp

    try
    {
-      tf_->transformPose(frame_id_, input_pose, out_pose);
+      // TODO: check logic here - which global collision body's transform should be used?
+      input_transform.setData(robot_state->getAttachedBody(contextIt->second->frame_id_)->getGlobalCollisionBodyTransforms()[0]);


Why not keep the previous code here?
input_transform.setData(robot_state->getLinkState(contextIt->second->frame_id_)->getGlobalCollisionBodyTransform());

Well, transfom_provider is never actually compiled, and if it were to be compiled, it would introduce a circular dependency between moveit_ros_perception and moveit_ros_planning, due to its use of planning_scene_monitor.

Additionally, it turns out the functiongetGlobalCollisionBodyTransform doesn't exist anywhere in the codebase. In order to make sure all of my other updates to this file were correct (tf2 message & type conversions, etc), I forced the compile of this file, which fails on this line. I put a band-aid on it to allow compiling, but it's likely not correct. I'm not sure what the proper procedure should be with this dead code.

rhaschke · 2018-05-03T21:26:14Z

moveit_ros/planning_interface/common_planning_interface_objects/src/common_objects.cpp

-  if (!s.tf_)
-    s.tf_.reset(new tf::TransformListener());
-  return s.tf_;
+  if (!s.tf_buffer_ || !s.tf_listener_)


Doesn't it suffice to check for listener? Both, buffer and listener, are always created together?

That is correct, both the buffer and the listener should be created together, and therefore just checking the for the listener should suffice. I structured this as such out of an abundance of caution in case the buffer is somehow deleted: the SharedStorage is a struct, making tf_buffer_ a public method. However, you are right that this logic can be simplified.

rhaschke · 2018-05-03T21:26:37Z

moveit_ros/planning_interface/common_planning_interface_objects/src/common_objects.cpp

-  return s.tf_;
+  if (!s.tf_buffer_ || !s.tf_listener_)
+  {
+    if(s.tf_listener_)


No need for this. Buffer is a shared_ptr anyway.

That's true, the buffer will continue to persist.

rhaschke · 2018-05-03T21:44:38Z

moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp

-        tf::matrixEigenToTF(current_state->getGlobalLinkTransform(lm).rotation(), ptf);
-        tfScalar pitch, roll, yaw;
-        ptf.getRPY(roll, pitch, yaw);
+        geometry_msgs::TransformStamped tfs = tf2::eigenToTransform(current_state->getGlobalLinkTransform(lm));


YPR can be directly computed from Eigen:

// yaw, pitch, roll corresponds to rotations about z, y, x axes, i.e. indeces 2, 1, 0 - in that order Eigen::Map<Eigen::Vector3d>(&result[0]) = current_state->getGlobalLinkTransform(lm).rotation().eulerAngles(2, 1, 0)

Well that's nifty. I'll update with your Eigen method.

@rhaschke wouldn't this line store the values in result as [yaw, pitch, roll] when result is expecting [roll, pitch, yaw]?

Damn. Haven't seen that. Indeed, this would required to switch roll and yaw or to reverse() the result of eulerAngles():
Eigen::Map<Eigen::Vector3d>(&result[0]) = current_state->getGlobalLinkTransform(lm).rotation().eulerAngles(2, 1, 0).reverse()

However, I'm completely fine, if you decide to keep your code for better readibility.

I'll use your updated Eigen method. I think a comment will suffice for readability.

After fiddling with this for a bit, I think I will keep the original tf2 conversion code.

tfoote

Reading this through it looks good. @rhaschke had a few suggestions that looked good too. I'm not likely to have a chance to run through again so +1

rhaschke

@IanTheEngineer In view of the upcoming Melodic release, I'm fine to skip the replacement of deprecated methods _addTransformsChangedListener and _getLatestCommonTime for now ;-)
However, you should check for Travis' complaints.

IanTheEngineer · 2018-05-17T12:25:37Z

@rhaschke I'm actually pretty close to fixing _getLatestCommonTime... I'll have some changes up soon.

- All type conversions now depend on geometry2 ROS packages, rather than geometry (see ros/geometry2#292 and ros/geometry2#294 for details of the new conversions) - Removes all boost::shared_ptr<tf::TransformListener> from the API, and replaced them with std::shared_ptr<tf2_ros::Buffer>'s - Piped tf2_ros::Buffer's everywhere where tf::TransformListeners were used - Utilizes the new tf2 API in the tf::Transformer library to access the internal tf2::Buffer object used by RViz (see ros/geometry#163 for details of the new API) - Removes the prepending of forward slashes ('/') for Transforms frames as this is deprecated in tf2

IanTheEngineer · 2018-05-17T14:15:30Z

I squashed most of the history of this PR, but it's still not quite ready for merging. I still have to

re-run clang cleanup
finish addressing @rhaschke 's review comments

As it would require fully grokking how TransformableRequest interface works, I am tempted to hold off on fixing the deprecated _addTransformsChangedListener for now, in the interest of getting a release out. I can solve that in a separate PR later on.

IanTheEngineer · 2018-05-17T17:53:34Z

At this point, I think the only reason Travis is failing is that it's attempting to build with kinetic.

IanTheEngineer · 2018-05-17T18:40:08Z

@rhaschke thanks for the fix - seems I incorrectly melded that file.

rhaschke · 2018-05-17T18:59:10Z

I'm currently looking into Travis / docker to fix our CI for melodic.

bmagyar · 2018-05-15T14:29:07Z

...ng_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h

                     const ros::WallDuration& wait_for_servers = ros::WallDuration());
-  MOVEIT_DEPRECATED MoveGroupInterface(const Options& opt, const boost::shared_ptr<tf::Transformer>& tf,
+  MOVEIT_DEPRECATED MoveGroupInterface(const Options& opt, const std::shared_ptr<tf2_ros::Buffer>& tf_buffer,


Isn't it time to drop everything marked with MOVEIT_DEPRECATED instead of migrating them?

Indeed, we should have a check for deprecated stuff. But we should carefully check, when and why we deprecated. I would only remove stuff that was deprecated in Indigo.

The point of keeping them with the deprecated flag was that they remain unchanged. If we keep updating their signature, the original functions are gone regardless.

I think the two scallywags below should be removed instead of updating them even though they were not deprecated already in Indigo.

I have no preference on updating or removing. However, for the purposes of preventing this PR from becoming too big and unwieldy, I tried to use a light touch in only updating what I absolutely had to update for tf2 to work.
I think another PR is in order for removing all MOVEIT_DEPRECATED functions from the Lunar/Kinetic release for Melodic.

... which I see you just recommended in #902 (comment). Appears we're on the same page @bmagyar :)

rhaschke · 2018-05-18T18:31:44Z

I approved that this compiles for Melodic and I'm going to squash-merge now.

IanTheEngineer · 2018-05-18T18:35:52Z

I'm fine with that, all of the changes are now in. Would you prefer that I squash everything manually in order to preserve the (hopefully informative) commit message?

IanTheEngineer · 2018-05-18T18:39:00Z

Nevermind! Looks like you handled it beautifully. Thanks @rhaschke! And thanks to everyone else for the reviews and feedback. It was a nice birthday present to have this merged today 🎂 😄

IanTheEngineer requested review from rhaschke, v4hn, davetcoleman and dirk-thomas April 9, 2018 01:50

IanTheEngineer added this to the ROS Melodic Release milestone Apr 9, 2018

IanTheEngineer requested a review from tfoote April 9, 2018 01:54

IanTheEngineer mentioned this pull request Apr 9, 2018

tf2 migration moveit/moveit_visual_tools#24

Merged

davetcoleman mentioned this pull request Apr 9, 2018

Release to ROS Melodic #806

Closed

25 tasks

davetcoleman changed the base branch from kinetic-devel to melodic-devel April 11, 2018 04:53

davetcoleman requested changes Apr 11, 2018

View reviewed changes

IanTheEngineer mentioned this pull request Apr 20, 2018

Add tf2 Buffer accessor in FrameManager ros-visualization/rviz#1224

Closed

IanTheEngineer commented Apr 20, 2018

View reviewed changes

IanTheEngineer force-pushed the tf2_migration branch 3 times, most recently from 33e4c48 to 1ea1e00 Compare April 27, 2018 22:02

IanTheEngineer mentioned this pull request May 1, 2018

Maintain & expose tf2 Buffer in shared_ptr for tf ros/geometry#163

Merged

IanTheEngineer force-pushed the tf2_migration branch from 423ee0c to 3703897 Compare May 3, 2018 17:34

IanTheEngineer mentioned this pull request May 3, 2018

upgrade to tf2 #745

Closed

rhaschke requested changes May 3, 2018

View reviewed changes

tfoote approved these changes May 4, 2018

View reviewed changes

IanTheEngineer force-pushed the tf2_migration branch from 6864801 to 6de8b83 Compare May 16, 2018 19:46

rhaschke approved these changes May 17, 2018

View reviewed changes

IanTheEngineer and others added 3 commits May 17, 2018 10:01

use new tf2 API in rviz to avoid deprecation warning

363918e

Replaced deprecated tf2 _getLatestCommonTime

c09d6bb

IanTheEngineer force-pushed the tf2_migration branch from 6de8b83 to c09d6bb Compare May 17, 2018 14:04

IanTheEngineer added 2 commits May 17, 2018 11:21

Updates tf2 pointer checks based on review

3919c5d

tf2 migration clang updates

8c3f2d7

IanTheEngineer force-pushed the tf2_migration branch from 881ee93 to 8c3f2d7 Compare May 17, 2018 16:33

fix class_loader.hpp

1cdd09d

bmagyar reviewed May 17, 2018

View reviewed changes

rhaschke mentioned this pull request May 17, 2018

Cleanup + Fixes to Dockerfiles for Melodic #902

Closed

IanTheEngineer mentioned this pull request May 18, 2018

Additional tf2 Type Conversions ros/geometry2#292

Merged

rhaschke merged commit 736251d into moveit:melodic-devel May 18, 2018

IanTheEngineer mentioned this pull request May 29, 2018

Ability to build melodic-devel branch on ROS Kinetic #925

Closed

IanTheEngineer mentioned this pull request Oct 16, 2018

Building melodic-devel on ROS Kinetic #1090

Merged

3 tasks

This was referenced Oct 29, 2018

remove left-over tf artifacts #1170

Merged

cherry picking from Kinetic #1160

Merged

Lint visualization (#1141) #1144

Merged

davetcoleman mentioned this pull request Mar 15, 2019

[WIP] Planning_scene_monitor (current_state_monitor) AcutronicRobotics/moveit2#29

Closed

IanTheEngineer mentioned this pull request Jul 9, 2019

[tf_eigen] Intended use of toMsg specialization ros/geometry2#389

Open

JafarAbdi mentioned this pull request Jan 27, 2020

Port planning_scene_monitor to ROS2 moveit/moveit2#112

Merged

MoveIt! tf2 migration #830

MoveIt! tf2 migration #830

Conversation

IanTheEngineer commented Apr 9, 2018 • edited

Highlights:

Things left TODO:

Checklist

IanTheEngineer commented Apr 9, 2018

v4hn commented Apr 10, 2018 via email

davetcoleman left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

IanTheEngineer commented Apr 20, 2018

Choose a reason for hiding this comment

tfoote commented Apr 27, 2018

IanTheEngineer commented Apr 30, 2018

IanTheEngineer commented May 3, 2018

davetcoleman commented May 3, 2018

IanTheEngineer commented May 3, 2018

rhaschke commented May 3, 2018

IanTheEngineer commented May 3, 2018

rhaschke left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

IanTheEngineer May 4, 2018 • edited

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

tfoote left a comment

Choose a reason for hiding this comment

rhaschke left a comment

Choose a reason for hiding this comment

IanTheEngineer commented May 17, 2018 • edited

IanTheEngineer commented May 17, 2018 • edited

IanTheEngineer commented May 17, 2018

IanTheEngineer commented May 17, 2018

rhaschke commented May 17, 2018

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

IanTheEngineer May 18, 2018 • edited

Choose a reason for hiding this comment

rhaschke commented May 18, 2018

IanTheEngineer commented May 18, 2018 • edited

IanTheEngineer commented May 18, 2018 • edited

IanTheEngineer commented Apr 9, 2018 •

edited

IanTheEngineer May 4, 2018 •

edited

IanTheEngineer commented May 17, 2018 •

edited

IanTheEngineer commented May 17, 2018 •

edited

IanTheEngineer May 18, 2018 •

edited

IanTheEngineer commented May 18, 2018 •

edited

IanTheEngineer commented May 18, 2018 •

edited